#!/usr/bin/env python3
# encoding: utf-8
# @Author: Zecous
# @Date: 2025/07/17
import cv2
import numpy as np
import rospy
import signal
import time
import math
from sensor_msgs.msg import Image
from std_msgs.msg import String, Bool
from std_srvs.srv import Trigger, TriggerResponse
from cv_bridge import CvBridge
from ainex_sdk import misc, common
from ainex_example.color_common import Common

class RedObjectFollower(Common):
    def __init__(self, name):
        # 初始化头部位置
        self.head_pan_init = 500   # 左右舵机的初始值
        self.head_tilt_look_down = 280  # 低头位置
        
        # 初始化ROS节点
        rospy.init_node(name)
        self.name = name
        self.running = True
        self.active = False  # 默认不激活，等待外部启动
        
        super().__init__(name, self.head_pan_init, self.head_tilt_look_down)
        
        # 设置退出处理函数
        signal.signal(signal.SIGINT, self.shutdown)
        
        # Initialize CV Bridge
        self.bridge = CvBridge()
        
        # State variables
        self.current_image = None
        self.current_state = "searching"  # searching -> approaching -> following -> finished
        self.lost_object_counter = 0  # 物体丢失计数器
        self.final_steps_counter = 0  # 最终步数计数器
        self.task_completed = False
        
        # 从ROS参数服务器加载配置
        self.load_config()
        
        # 图像参数
        self.image_width = rospy.get_param('~image_width', 640)
        self.image_height = rospy.get_param('~image_height', 480)

        # 检测稳定性状态（先设置默认值，后续根据配置更新）
        self.frames_processed = 0
        self.consecutive_valid_frames = 0
        self.detection_warmup_frames = 6
        self.detection_stability_frames = 3
        self.min_red_area = 400
        self.max_red_area_ratio = 0.4
        self.roi_y_min_ratio = 0.0
        self.roi_y_max_ratio = 1.0
        self.roi_y_min = 0
        self.roi_y_max = self.image_height
        
        # 从配置加载位置判断参数
        self.load_position_params()

        # 加载检测相关参数
        self.load_detection_params()
        
        # 初始化运动参数
        self.init_movement_params()
        
        # ROS通信接口
        self.setup_ros_interface()

        # 显示窗口控制
        self.update_display_config()
        
        # Subscribe to camera topic
        self.image_subscriber = rospy.Subscriber('/camera/image_raw', Image, self.image_callback)
        
        # 初始化机器人动作
        self.motion_manager.run_action('walk_ready')
        
        # 设置低头位置
        self.set_servo(self.head_pan_init, self.head_tilt_look_down)
        # time.sleep(2)  # 等待舵机到位
        
        common.loginfo('Red object follower initialized with ROS interface')
        print("Red object following task ready, waiting for activation...")

    def update_display_config(self):
        """从参数服务器更新显示配置"""
        display_ns = '/system/display'
        main_flag = rospy.get_param(f'{display_ns}/show_go_around_window', None)
        if main_flag is None:
            main_flag = rospy.get_param(f'{display_ns}/show_main_window', True)
        self.display_main_window = bool(main_flag)

        mask_flag = rospy.get_param(f'{display_ns}/show_detection_masks', True)
        self.display_mask_window = self.display_main_window and bool(mask_flag)

    @staticmethod
    def _safe_destroy_window(window_name):
        try:
            cv2.destroyWindow(window_name)
        except cv2.error:
            pass
        except Exception:
            pass

    def setup_ros_interface(self):
        """设置ROS通信接口"""
        # 状态发布器
        self.status_pub = rospy.Publisher('/go_around/status', String, queue_size=1)
        self.active_pub = rospy.Publisher('/go_around/active', Bool, queue_size=1)
        self.progress_pub = rospy.Publisher('/go_around/progress', String, queue_size=1)
        
        # 控制服务
        self.start_service = rospy.Service('/go_around/start', Trigger, self.start_service_callback)
        self.stop_service = rospy.Service('/go_around/stop', Trigger, self.stop_service_callback)
        self.reset_service = rospy.Service('/go_around/reset', Trigger, self.reset_service_callback)
        
        # 控制命令订阅
        self.control_sub = rospy.Subscriber('/go_around/control', String, self.control_callback)

    def start_service_callback(self, req):
        """启动服务回调"""
        self.active = True
        self.running = True
        self.task_completed = False
        self.current_state = "searching"
        self.lost_object_counter = 0
        self.final_steps_counter = 0
        self.publish_status("started")
        self.reset_detection_state()
        return TriggerResponse(success=True, message="Go around task started")

    def stop_service_callback(self, req):
        """停止服务回调"""
        self.active = False
        self.gait_manager.stop()
        self.reset_detection_state()
        self.publish_status("stopped")
        return TriggerResponse(success=True, message="Go around task stopped")

    def reset_service_callback(self, req):
        """重置服务回调"""
        self.active = False
        self.gait_manager.stop()
        self.current_state = "searching"
        self.lost_object_counter = 0
        self.final_steps_counter = 0
        self.task_completed = False
        self.reset_detection_state()
        self.publish_status("reset")
        return TriggerResponse(success=True, message="Go around task reset")

    def control_callback(self, msg):
        """控制命令回调"""
        command = msg.data.lower()
        if command == "start":
            self.active = True
            self.running = True
            self.task_completed = False
            self.reset_detection_state()
        elif command == "stop":
            self.active = False
            self.gait_manager.stop()
            self.reset_detection_state()
        elif command == "reset":
            self.active = False
            self.gait_manager.stop()
            self.current_state = "searching"
            self.lost_object_counter = 0
            self.final_steps_counter = 0
            self.task_completed = False
            self.reset_detection_state()

    def publish_status(self, status):
        """发布状态"""
        msg = String()
        msg.data = status
        self.status_pub.publish(msg)
        
        active_msg = Bool()
        active_msg.data = self.active
        self.active_pub.publish(active_msg)
        
        progress_msg = String()
        progress_msg.data = f"State: {self.current_state}, Completed: {self.task_completed}"
        self.progress_pub.publish(progress_msg)

    def load_config(self):
        """从ROS参数服务器加载配置"""
        try:
            # 从参数服务器获取颜色配置
            red_config = rospy.get_param('/image_processing/colors/red', {})
            self.red_lower = np.array(red_config.get('lower', [44, 138, 130]))
            self.red_upper = np.array(red_config.get('upper', [163, 196, 255]))
            
            # 从参数服务器获取舵机配置
            servo_config = rospy.get_param('/servo', {})
            self.head_pan_init = servo_config.get('head_pan_init', 500)
            self.head_tilt_look_down = servo_config.get('head_tilt_look_down', 280)
            
            # print(f"配置加载成功 - 红色范围: {self.red_lower} ~ {self.red_upper}")
            
        except Exception as e:
            print(f"配置加载失败，使用默认值: {e}")
            # 使用默认配置
            self.red_lower = np.array([44, 138, 130])
            self.red_upper = np.array([163, 196, 255])
            self.head_pan_init = 500
            self.head_tilt_look_down = 280

    def load_position_params(self):
        """从配置加载位置判断参数"""
        try:
            # 从参数服务器获取绕行任务配置
            go_around_config = rospy.get_param('/tasks/go_around', {})
            position_ratios = go_around_config.get('position_ratios', {})
            movement_config = go_around_config.get('movement', {})
            
            # 加载位置比例参数
            target_x_ratio = position_ratios.get('target_x_ratio', 0.75)
            right_edge_ratio = position_ratios.get('right_edge_ratio', 0.9)
            self.target_x_position = int(self.image_width * target_x_ratio)
            self.right_edge_threshold = int(self.image_width * right_edge_ratio)
            
            # 加载运动参数
            self.lost_object_timeout = movement_config.get('lost_object_timeout', 3.0)
            self.final_steps = movement_config.get('final_steps', 5)
            self.final_turn_yaw = movement_config.get('final_turn_yaw', -5)
            self.final_turn_steps = movement_config.get('final_turn_steps', 1)
            self.final_turn_forward = movement_config.get('final_turn_forward', 0.0)
            self.final_shift_y = movement_config.get('final_shift_y', -0.006)
            self.final_followup_yaw = movement_config.get('final_followup_yaw', -4)
            self.final_forward_after_turn = movement_config.get('final_forward_after_turn', 0.015)
            self.final_forward_steps = movement_config.get('final_forward_steps', 4)
            
            print(f"位置参数加载成功 - go_around_config: {go_around_config}")
            print(f"位置参数加载成功 - 目标X位置: {self.target_x_position} (比例: {target_x_ratio})")
            print(f"右侧边缘阈值: {self.right_edge_threshold} (比例: {right_edge_ratio})")
            print(f"运动参数: 超时时间: {self.lost_object_timeout}秒, 最终步数: {self.final_steps}")
            print(f"右转参数: 角度: {self.final_turn_yaw}, 步数: {self.final_turn_steps}")
            
        except Exception as e:
            print(f"位置参数加载失败，使用默认值: {e}")
            # 使用默认配置
            self.target_x_position = int(self.image_width * 0.75)
            self.right_edge_threshold = int(self.image_width * 0.9)
            self.lost_object_timeout = 3.0
            self.final_steps = 5
            self.final_turn_yaw = -5
            self.final_turn_steps = 1
            self.final_turn_forward = 0.0
            self.final_shift_y = -0.006
            self.final_followup_yaw = -4
            self.final_forward_after_turn = 0.015
            self.final_forward_steps = 4

    def load_detection_params(self):
        """加载检测相关参数"""
        try:
            detection_defaults = rospy.get_param('/image_processing/detection', {})
            go_around_config = rospy.get_param('/tasks/go_around', {})
            detection_config = go_around_config.get('detection', {})

            self.min_red_area = detection_config.get(
                'min_area',
                detection_defaults.get('min_area', self.min_red_area)
            )
            self.max_red_area_ratio = detection_config.get(
                'max_area_ratio',
                detection_defaults.get('max_area_ratio', self.max_red_area_ratio)
            )
            self.detection_stability_frames = detection_config.get(
                'stability_frames',
                detection_defaults.get('stability_frames', self.detection_stability_frames)
            )
            self.detection_warmup_frames = detection_config.get(
                'warmup_frames', self.detection_warmup_frames
            )

            roi_config = detection_config.get('roi', {})
            self.roi_y_min_ratio = float(roi_config.get('y_min', self.roi_y_min_ratio))
            self.roi_y_max_ratio = float(roi_config.get('y_max', self.roi_y_max_ratio))

        except Exception as e:
            rospy.logwarn(f"Failed to load go_around detection params, using defaults: {e}")

        # 规范化参数
        self.detection_stability_frames = max(1, int(self.detection_stability_frames))
        self.detection_warmup_frames = max(0, int(self.detection_warmup_frames))
        self.min_red_area = max(1, int(self.min_red_area))
        self.max_red_area_ratio = max(0.0, min(1.0, float(self.max_red_area_ratio)))
        self.roi_y_min_ratio = max(0.0, min(1.0, float(self.roi_y_min_ratio)))
        self.roi_y_max_ratio = max(self.roi_y_min_ratio + 0.01, min(1.0, float(self.roi_y_max_ratio)))

        self.roi_y_min = int(self.image_height * self.roi_y_min_ratio)
        self.roi_y_max = int(self.image_height * self.roi_y_max_ratio)
        self.roi_y_min = max(0, min(self.roi_y_min, self.image_height - 1))
        self.roi_y_max = max(self.roi_y_min + 1, min(self.roi_y_max, self.image_height))

    def reset_detection_state(self):
        """重置检测状态，避免历史误差影响"""
        self.frames_processed = 0
        self.consecutive_valid_frames = 0

    def init_movement_params(self):
        """初始化运动参数"""
        self.calib_config = common.get_yaml_data('/home/ubuntu/ros_ws/src/ainex_example/config/calib.yaml')
        
        self.go_gait_param = self.gait_manager.get_gait_param()
        self.go_gait_param["angle_move_amplitude"] = -4.0
        self.go_gait_param["step_height"] = 0.018  # z_move_amplitude
        self.go_gait_param["z_swap_amplitude"] = 0.004
        self.go_gait_param["init_roll_offset"] = -4
        self.go_gait_param["init_pitch_offset"] = -4.0
        # self.go_gait_param["init_y_offset"] = -0.005
        self.go_gait_param["step_fb_ratio"] = 0.028
        self.go_gait_param["pelvis_offset"] = 2  # 减小髋关节摆动，提高稳定性
        self.go_gait_param['hip_pitch_offset'] = 5.5
        self.go_dsp=[280, 0.18, 0.025]
        self.go_arm_swap = 35
        
        # 转弯步态参数
        self.turn_gait_param = self.gait_manager.get_gait_param()
        self.turn_gait_param["angle_move_amplitude"] = -4.0
        self.turn_gait_param["step_height"] = 0.022  # 转弯时增加步高，提高转弯效果
        self.turn_gait_param["z_swap_amplitude"] = 0.006  # 增加摆动幅度
        self.turn_gait_param["init_roll_offset"] = -5
        self.turn_gait_param["init_pitch_offset"] = -4.0
        # self.turn_gait_param["init_y_offset"] = -0.005
        self.turn_gait_param["step_fb_ratio"] = 0.020  # 减小前后比例，增加侧向转弯
        self.turn_gait_param["pelvis_offset"] = 4  # 转弯时稍微增加髋关节摆动
        self.turn_gait_param['hip_pitch_offset'] = 3  # 减少前倾，增加稳定性

        self.turn_dsp = [250, 0.15, 0.03]  # 转弯时更快步频、更小双足支撑、更大摆幅
        self.turn_arm_swap = 45
        self.x_max = 0.018  # 最大前进步长
        self.yaw_range = [-50, 50]  # 转弯范围
        

    def image_callback(self, msg):
        """ROS图像回调函数"""
        try:
            self.current_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
        except Exception as e:
            rospy.logerr(f"Failed to convert image: {e}")

    def detect_red_object(self, image):
        """检测红色物体"""
        if image is None:
            return None, None
        
        # 转换为LAB色彩空间
        lab = cv2.cvtColor(image, cv2.COLOR_BGR2LAB)

        # 创建红色掩码
        mask = cv2.inRange(lab, self.red_lower, self.red_upper)

        # 应用ROI限制，减少背景干扰
        if self.roi_y_min > 0 or self.roi_y_max < mask.shape[0]:
            roi_mask = np.zeros_like(mask)
            roi_mask[self.roi_y_min:self.roi_y_max, :] = 255
            mask = cv2.bitwise_and(mask, roi_mask)

        # 形态学操作去除噪声
        kernel = np.ones((5, 5), np.uint8)
        mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
        mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)
        
        # 查找轮廓
        contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
        
        if len(contours) > 0:
            # 找到最大的轮廓
            largest_contour = max(contours, key=cv2.contourArea)
            area = cv2.contourArea(largest_contour)
            
            # 过滤掉太小或过大的物体
            effective_area = max(1, (self.roi_y_max - self.roi_y_min) * self.image_width)

            if area < self.min_red_area:
                self.consecutive_valid_frames = 0
                return None, mask

            area_ratio = area / float(effective_area)
            if area_ratio > self.max_red_area_ratio:
                print(f"Ignoring red area due to excessive size ratio: {area_ratio:.2f}")
                self.consecutive_valid_frames = 0
                return None, mask

            # 检查是否过早（曝光尚未稳定）
            if self.frames_processed <= self.detection_warmup_frames:
                self.consecutive_valid_frames = 0
                return None, mask

            # 计算物体中心
            M = cv2.moments(largest_contour)
            if M["m00"] != 0:
                cx = int(M["m10"] / M["m00"])
                cy = int(M["m01"] / M["m00"])

                # 连续检测计数
                self.consecutive_valid_frames += 1
                if self.consecutive_valid_frames < self.detection_stability_frames:
                    return None, mask

                return (cx, cy), mask
        
        # 未检测到有效物体
        self.consecutive_valid_frames = 0
        return None, mask

    def control_robot_movement(self, object_center):
        """根据状态和物体位置控制机器人移动"""
        if not self.active:  # 如果任务未激活，不执行移动
            return
            
        if object_center is None:
            self.handle_lost_object()
            return
        
        # 重置丢失计数器
        self.lost_object_counter = 0
        
        cx, cy = object_center
        print(f"Red object at ({cx}, {cy}), state: {self.current_state}")
        
        if self.current_state == "searching":
            self.approach_object(cx)
        elif self.current_state == "approaching":
            self.approach_to_target_position(cx)
        elif self.current_state == "following":
            self.follow_object_on_right(cx)

    def approach_object(self, cx):
        """阶段1：向红色物体靠近（始终向左前方走）"""
        # 不管物体在哪里，都向左前方走，直到物体到达画面右侧位置
        print("Approaching object - moving left-forward with stronger lateral movement...")
        self.gait_manager.set_step(self.go_dsp, 0.008, 0.010, 4, 
                                 self.go_gait_param, arm_swap=self.go_arm_swap, step_num=0)
        
        # 检查是否到达目标位置（物体在画面右侧位置）
        print(f"[DEBUG] 检查是否到达目标位置: cx={cx}, target_x_position={self.target_x_position}, 条件: {cx >= self.target_x_position}")
        if cx >= self.target_x_position:
            print("Object reached target position, switching to following mode...")
            self.current_state = "following"
            self.publish_status("following")

    def approach_to_target_position(self, cx):
        """阶段2：调整到目标位置"""
        # 这个方法现在整合到approach_object中了
        pass

    def follow_object_on_right(self, cx):
        """阶段3：保持物体在画面最右侧跟随"""
        if cx >= self.right_edge_threshold:
            # 物体太靠右，右转角度更大
            print("Object too far right, turning right with stronger angle...")
            yaw_output = -6
            x_output = 0.008
        elif cx < self.target_x_position:
            # 物体偏左了，左转调整 
            print("Object moved left, adjusting left...")
            yaw_output = 20
            x_output = 0.014
        else:
            # 物体位置合适，直行
            print("Following object straight...")
            yaw_output = -4
            x_output = self.x_max
       
        print("YAW:= ",yaw_output)
        # 执行连续移动
        if abs(yaw_output) <= 5 or yaw_output >= 0:
            self.gait_manager.set_step(self.go_dsp, x_output, 0, int(yaw_output), 
                                     self.go_gait_param, arm_swap=self.go_arm_swap, step_num=0)
        else:
            self.gait_manager.set_step(self.turn_dsp, x_output, 0, int(yaw_output), 
                                     self.turn_gait_param, arm_swap=self.turn_arm_swap, step_num=0)

    def handle_lost_object(self):
        """处理物体丢失的情况"""
        self.lost_object_counter += 1
        
        if self.current_state == "following" and self.lost_object_counter > 10:
            # 在跟随状态下丢失物体，进入最终阶段
            print("Object lost during following, starting final steps...")
            self.current_state = "finished"
            self.execute_final_steps()
        elif self.lost_object_counter > 5:
            # 长时间找不到物体，停止
            print("Object lost for too long, stopping...")
            self.gait_manager.stop()
            self.publish_status("object_lost")
        else:
            # 继续寻找，稍微前进
            print(f"Object lost, searching... ({self.lost_object_counter})")
            if self.current_state == "searching":
                self.gait_manager.set_step(self.go_dsp, 0.005, 0, 0, 
                                         self.go_gait_param, arm_swap=self.go_arm_swap, step_num=0)

    def execute_final_steps(self):
        """执行最终动作"""
        if self.final_steps_counter == 0:
            print("Executing final turn in place")
            self.gait_manager.set_step(
                self.turn_dsp,
                self.final_turn_forward,
                0.0,
                int(self.final_turn_yaw),
                self.turn_gait_param,
                arm_swap=self.turn_arm_swap,
                step_num=max(1, self.final_turn_steps)
            )
            self.final_steps_counter = 1
            self.publish_status("final_turn")
        elif self.final_steps_counter == 1:
            print("Shifting right after turn")
            self.gait_manager.set_step(
                self.turn_dsp,
                max(0.0, self.final_turn_forward),
                self.final_shift_y,
                int(self.final_followup_yaw),
                self.turn_gait_param,
                arm_swap=self.turn_arm_swap,
                step_num=max(1, self.final_turn_steps - 1)
            )
            self.final_steps_counter = 2
            self.publish_status("final_shift")
        elif self.final_steps_counter == 2:
            print("Advancing to clear obstacle")
            self.gait_manager.set_step(
                self.go_dsp,
                self.final_forward_after_turn,
                self.final_shift_y,
                int(self.final_followup_yaw),
                self.turn_gait_param,
                arm_swap=self.turn_arm_swap,
                step_num=max(1, self.final_forward_steps)
            )
            self.final_steps_counter = 3
            self.publish_status("final_forward")
        elif self.final_steps_counter == 3:
            print("Go around mission completed! Stopping robot.")
            self.gait_manager.stop()
            self.task_completed = True
            self.active = False
            self.publish_status("completed")
            self.final_steps_counter = 4

    def set_servo(self, pan, tilt):
        """设置舵机位置"""
        try:
            self.motion_manager.set_servos_position(200, [[23, pan], [24, tilt]])
        except Exception as e:
            rospy.logerr(f"Servo control error: {e}")

    def shutdown(self, signum, frame):
        """节点关闭回调函数"""
        with self.lock:
            self.motion_manager.run_action('stand')
            self.running = False 
            self.active = False
            self.publish_status("shutdown")
            common.loginfo('%s shutdown' % self.name)

    def run(self):
        """主循环函数"""
        print("Waiting for camera image...")
        
        # 等待图像数据
        while self.current_image is None and self.running:
            rospy.sleep(0.1)
        
        print("Camera ready, go around task standby...")
        self.publish_status("ready")
        
        # 窗口显示状态
        windows_created = False
        mask_window_created = False
        
        while self.running and not rospy.is_shutdown():
            try:
                # 支持运行时更新显示配置
                self.update_display_config()
                if not self.display_main_window and windows_created:
                    self._safe_destroy_window('Red Object Follower - ACTIVE')
                    windows_created = False
                if (not self.display_mask_window or not self.display_main_window) and mask_window_created:
                    self._safe_destroy_window('Red Mask - ACTIVE')
                    mask_window_created = False

                if self.current_image is not None and self.active:
                    self.frames_processed += 1
                    # 检测红色物体
                    object_center, mask = self.detect_red_object(self.current_image)
                    
                    # 创建显示图像
                    display_image = self.current_image.copy()
                    
                    if object_center is not None:
                        cx, cy = object_center
                        # 在图像上绘制物体中心
                        cv2.circle(display_image, (cx, cy), 10, (0, 255, 0), -1)
                        
                        # 绘制目标位置线和右边缘线
                        cv2.line(display_image, (self.target_x_position, 0), 
                                (self.target_x_position, self.image_height), (255, 0, 0), 2)
                        cv2.line(display_image, (self.right_edge_threshold, 0), 
                                (self.right_edge_threshold, self.image_height), (0, 0, 255), 2)
                        
                        # 显示状态信息
                        cv2.putText(display_image, f"STATE: {self.current_state.upper()}", (10, 30), 
                                  cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
                        cv2.putText(display_image, f"OBJ_X: {cx}", (10, 60), 
                                  cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
                        cv2.putText(display_image, "GO AROUND - ACTIVE", (10, 90), 
                                  cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
                        
                        # 控制机器人移动
                        if not self.task_completed:
                            self.control_robot_movement(object_center)
                    else:
                        cv2.putText(display_image, "NO RED OBJECT", (10, 30), 
                                  cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
                        cv2.putText(display_image, f"STATE: {self.current_state.upper()}", (10, 60), 
                                  cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
                        cv2.putText(display_image, "GO AROUND - ACTIVE", (10, 90), 
                                  cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
                        
                        # 处理物体丢失
                        if not self.task_completed:
                            self.control_robot_movement(None)
                    
                    # 显示任务状态
                    if self.task_completed:
                        cv2.putText(display_image, "TASK COMPLETED", (10, 120), 
                                  cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 255), 2)
                    
                    # 处理最终步骤
                    if self.current_state == "finished" and not self.task_completed:
                        self.execute_final_steps()
                    
                    # 显示图像（只在激活时）
                    if self.display_main_window:
                        cv2.imshow('Red Object Follower - ACTIVE', display_image)
                        windows_created = True
                        if mask is not None and self.display_mask_window:
                            cv2.imshow('Red Mask - ACTIVE', mask)
                            mask_window_created = True
                        elif mask_window_created and (mask is None or not self.display_mask_window):
                            self._safe_destroy_window('Red Mask - ACTIVE')
                            mask_window_created = False

                        # 检查退出键
                        key = cv2.waitKey(30) & 0xFF
                        if key == 27:  # ESC键
                            break
                    else:
                        rospy.sleep(0.05)
                        
                elif not self.active and windows_created:
                    # 任务未激活时关闭窗口
                    self._safe_destroy_window('Red Object Follower - ACTIVE')
                    if mask_window_created:
                        self._safe_destroy_window('Red Mask - ACTIVE')
                        mask_window_created = False
                    windows_created = False
                    rospy.sleep(0.1)
                else:
                    rospy.sleep(0.1)
                    
            except KeyboardInterrupt:
                break
            except Exception as e:
                print(f"Error in main loop: {e}")
                rospy.sleep(0.1)
        
        # 退出前动作
        self.gait_manager.stop()
        # self.set_servo(self.head_pan_init, self.head_tilt_look_down)
        self.publish_status("stopped")
        if windows_created:
            self._safe_destroy_window('Red Object Follower - ACTIVE')
        if mask_window_created:
            self._safe_destroy_window('Red Mask - ACTIVE')
        rospy.signal_shutdown('shutdown')

if __name__ == '__main__':
    try:
        follower = RedObjectFollower('red_object_follower')
        follower.run()
    except Exception as e:
        print(f"Failed to start follower: {e}")
        rospy.logerr(f"Follower initialization error: {e}")
